/*
 * AppConfig.cpp
 *
 *  Created on: 2010-03-30
 *      Author: kamil
 */

#include "mgr/config/Config.h"
//#include "field/Field.h"
#include "mgr/id/IdMgr.h"
//#include "robot/Robot.h"
#include <boost/foreach.hpp>
#include <boost/tokenizer.hpp>

namespace po = boost::program_options;
using namespace std;

Config & Config::instance(){
	static Config instance;
	return instance;
}

void Config::parse(std::string fileName){
	BOOST_ASSERT(!fileName.empty());
	string teamBlue, teamYellow;

	//field data
	double fieldW = 0,fieldH = 0;
	double areaW = 0, areaH = 0;
	double firstGoalX = 0, firstGoalY = 0;
	double secondGoalX = 0, secondGoalY = 0;

	po::options_description config_file_options("Autonomic client config file options");
	config_file_options.add_options()
	    ("teams.team_yellow", po::value<string>(&teamYellow), "Model names for team yellow")
	    ("teams.team_blue", po::value<string>(&teamBlue), "Model names for team blue")

	    ("ball.name",  po::value<string>(&ballInfo.name), "Ball model name")
	    ("ball.radius",  po::value<double>(&ballInfo.radius), "Ball model radius")

	    ("robot.radius",  po::value<double>(&robotInfo.radius), "Robot model radius")
	    ("robot.kicking_force",  po::value<double>(&robotInfo.kickingForce), "Robot kicking force")
	    ("robot.passing_force", po::value<double>(&robotInfo.passingForce), "Robot passing force")

	    ("field.size_x",  po::value<double>(&fieldW), "Field size along x axis")
	    ("field.size_y",  po::value<double>(&fieldH), "Field size along y axis")

	    ("field.area_size_x",  po::value<double>(&areaW), "Game area size along x axis")
	    ("field.area_size_y",  po::value<double>(&areaH), "Game area size along y axis")

	    ("field.first_goal_pos_x",  po::value<double >(&firstGoalX), "First goal x pos")
	    ("field.first_goal_pos_y",  po::value<double >(&firstGoalY), "First goal y pos")

	    ("field.second_goal_pos_x",  po::value<double >(&secondGoalX), "Second goal x pos")
	    ("field.second_goal_pos_y",  po::value<double >(&secondGoalY), "Second goal y pos")

	    ("field.goal_width",  po::value<double >(&fieldInfo.goalWidth), "Goal width")

	    ("field.center_circle_radius",  po::value<double >(&fieldInfo.centerCircleRadius), "Center circle radius")

	    ("rrt.safety_margin",  po::value<double>(&rrtInfo.safetyMargin), "Safety margin for obstacles used in RRT alg.")
	    ("rrt.max_nodes",  po::value<int>(&rrtInfo.maxNodes), "Max number of nodes used by RRT alg.")

	    ("prediction.steps",  po::value<int>(&predictionInfo.steps), "Number of steps forward used in prediction")
	    ("prediction.time",  po::value<double>(&predictionInfo.time), "Time between steps in prediction")

	    ("rotation_pid.k", po::value<double>(&rotationPidInfo.k), "Rotation PID, K parameter")
	    ("rotation_pid.ti", po::value<double>(&rotationPidInfo.ti), "Rotation PID, ti parameter")
	    ("rotation_pid.td", po::value<double>(&rotationPidInfo.td), "Rotation PID, td parameter")

	    ("position_pid.k", po::value<double>(&positionPidInfo.k), "Position PID, K parameter")
	    ("position_pid.ti", po::value<double>(&positionPidInfo.ti), "Position PID, ti parameter")
	    ("position_pid.td", po::value<double>(&positionPidInfo.td), "Position PID, td parameter")

	    ("video.update_rate", po::value<double>(&videoInfo.updateRate), "Videoserver info, update rate in seconds")
	;

	po::variables_map vm;
	ifstream ifs(fileName.c_str());
	po::store(parse_config_file(ifs, config_file_options), vm);
	po::notify(vm);

	fieldInfo.fieldArea = Rect(0,0,fieldW, fieldH);
	Vector2d offset((fieldW - areaW) / 2.0, (fieldH - areaH) / 2.0);
	fieldInfo.gameArea = Rect(offset.x, offset.y, areaW, areaH);
	fieldInfo.firstGoal = Vector2d(firstGoalX, firstGoalY);
	fieldInfo.secondGoal = Vector2d(secondGoalX,secondGoalY);
	fieldInfo.center.x = fieldW / 2.0; fieldInfo.center.y = fieldH / 2.0;

	ballInfo.id = IdMgr::instance().getId(ballInfo.name);

	prepareTeamMap(teamBlue, teamYellow);

#if 0
	std::cout<<"****** Config *********"<<std::endl;
	std::cout<<ballInfo<<std::endl;
	std::cout<<robotInfo<<std::endl;
	std::cout<<rrtInfo<<std::endl;
	std::cout<<predictionInfo<<std::endl;
	std::cout<<fieldInfo<<std::endl;
#endif
}

Config::Config() {

}

Config::~Config() {

}

void Config::prepareTeamMap(const std::string & teamBlue, const std::string & teamYellow)
{
	ModelId id;
	_teamMap.clear();
	boost::char_separator<char> sep(" ");

	boost::tokenizer<boost::char_separator<char> > t1Tokens(teamBlue,sep);
	BOOST_FOREACH(string t, t1Tokens){
		id = IdMgr::instance().getId(t);
		_teamMap.insert(std::make_pair(id, TEAM_BLUE));
		_teamBlue.insert(id);
	}

	boost::tokenizer<boost::char_separator<char> > t2Tokens(teamYellow,sep);
	BOOST_FOREACH(string t, t2Tokens){
		id = IdMgr::instance().getId(t);
		_teamMap.insert(std::make_pair(id, TEAM_YELLOW));
		_teamYellow.insert(id);
	}
}

TeamName Config::getTeamName(const ModelId & id){
	return _teamMap[id];
}

void Config::getTeammates(const ModelId & id, std::list<ModelId> * teammates){
	TeamName name = getTeamName(id);
	std::set<ModelId> teamSet;
	if (name == TEAM_BLUE)
		teamSet = _teamBlue;
	else
		teamSet = _teamYellow;

	set<ModelId>::iterator it = teamSet.find(id);
	BOOST_ASSERT(it != teamSet.end());
	teamSet.erase(id);	//remove robot from teamset - there should be teammates, not whole team

	teammates->clear();
	for(it = teamSet.begin(); it != teamSet.end(); it++)
		teammates->push_back(*it);
}

void Config::getTeam(const TeamName & teamName, std::list<ModelId> * team){
	std::set<ModelId> * teamSet;
	if (teamName == TEAM_BLUE)
		teamSet = &_teamBlue;
	else
		teamSet = &_teamYellow;

	std::set<ModelId>::iterator it = teamSet->begin();
	for(; it != teamSet->end(); ++it)
		team->push_back(*it);
}

int Config::getTeamSize(){
	BOOST_ASSERT(_teamBlue.size() != 0);
	BOOST_ASSERT(_teamBlue.size() == _teamYellow.size());
	return  _teamBlue.size();
}

Vector2d Config::FieldInfo::getGoalPosition(GoalId id) const {
	if (id == GOAL_FIRST)
		return firstGoal;

	return secondGoal;
}

std::ostream & operator<<(std::ostream & s, const Config::BallInfo & i){
	s<<"Ball info: name="<<i.name<<" id="<<i.id<<" r="<<i.radius;
	return s;
}

std::ostream & operator<<(std::ostream & s, const Config::RobotInfo & i){
	s<<"Robot info: r="<<i.radius<<" kicking_force: "<<i.kickingForce<<" passing_force: "<<i.passingForce;
	return s;
}

std::ostream & operator<<(std::ostream & s, const Config::RRTInfo &i){
	s<<"RRT info: safetyMargin="<<i.safetyMargin<<" maxNodes="<<i.maxNodes;
	return s;
}

std::ostream & operator<<(std::ostream & s, const Config::PredictionInfo & i){
	s<<"Prediction info: steps="<<i.steps<<" time="<<i.time;
	return s;
}

std::ostream & operator<<(std::ostream & s, const Config::FieldInfo & i){
	s<<"FieldInfo:"<<std::endl;
	s<<"* fieldArea: "<<i.fieldArea<<std::endl;
	s<<"* gameArea: "<<i.gameArea<<std::endl;
	s<<"* firstGoal: "<<i.firstGoal<<std::endl;
	s<<"* secondGoal: "<<i.secondGoal<<std::endl;
	s<<"* goalWidth: "<<i.goalWidth;
	s<<"* centerCircleRadius: "<<i.centerCircleRadius;
	return s;
}

std::ostream & operator<<(std::ostream & s, const Config::PidInfo & i){
	s<<"PidInfo:"<<std::endl;
	s<<"* k="<<i.k<<std::endl;
	s<<"* ti="<<i.ti<<std::endl;
	s<<"* td="<<i.td<<std::endl;
	return s;
}

std::ostream & operator<<(std::ostream & s, const Config::VideoInfo & i){
	s<<"VideoInfo:"<<std::endl;
	s<<"* updateRate="<<i.updateRate<<std::endl;
	return s;
}

/*
/////////////////////////////////////////////////////////////////////////////////////////

std::string Config::videoserverIP(){
	return _videoserverIP;
}
int Config::videoserverPort(){
	return _videoserverPort;
}

std::string Config::ball(){
	return _ball;
}

ModelId Config::ballId(){
	return _ballId;
}

ModelId Config::robotId(){
	return _robotId;
}

double Config::ballRadius(){
	return _ballradius;
}

double Config::safetyMargin(){
	return _safetyMargin;
}

int Config::maxNodes(){
	return _maxNodes;
}

int Config::predictionSteps(){
	return _predictionSteps;
}

double Config::predictionTime(){
	return _predictionTime;
}*/
